package oldFiles;
import lejos.nxt.*;

public class BalancerPid {
	
	//PID constants
	
	static final int KP = 28;
	static final int KI = 4;
	static final int KD = 33;
	static final int SCALE = 18;
	
	//global vars
	int offset;
	int prev_error;
	float int_error;
	
	LightSensor ls;
	
	public BalancerPid() {
		ls = new LightSensor(SensorPort.S2,true);
		Motor.B.regulateSpeed(false);
		Motor.C.regulateSpeed(false);
	}
	
	public void getBalancePos() {
		//wait for user to balance and press enter
		while (!Button.ENTER.isPressed()) {
			//must be balanced here
			offset = ls.readNormalizedValue();
			LCD.clear();
			LCD.drawInt(offset, 2, 4);
			LCD.refresh();
		}
	}
	
	public void pidControl() {
		while (!Button.ESCAPE.isPressed()) {
			int normVal = ls.readNormalizedValue();
			
			// proportional error
			int error = normVal - offset;
			//adjust far and near readings
			if (error < 0) error = (int)(error*1.8F);
			
			// integral error
			int_error = ((int_error + error)*2)/3;
			
			//derivative error
			int deriv_error = error - prev_error;
			prev_error = error;
			
			int pid_val = (int)(KP * error + KI * int_error+KD*deriv_error)/SCALE;
			
			if (pid_val > 100) pid_val = 100;
			if (pid_val < -100) pid_val = -100;
			
			//power derived from pid values
			int power = Math.abs(pid_val);
			power = 55 + (power*45)/100; //normalize power
			Motor.B.setPower(power);
			Motor.C.setPower(power);
			
			if (pid_val > 0) {
				Motor.B.forward();
				Motor.C.forward();
			}
			else {
				Motor.B.backward();
				Motor.C.backward();
			}
			
		}
	}
	
	public void shutDown() {
		//shutdown light and motors
		Motor.B.flt();
		Motor.C.flt();
		ls.setFloodlight(false);
		
	}
	
	public static void main(String[] args) {
		
		BalancerPid bp = new BalancerPid();
		bp.getBalancePos();
		bp.pidControl();
		bp.shutDown();
	}
}
